% -------------------------------------------------------------------------
% plot transfer orbit
%
% plot_orbit_(p0 , auxdata)
%
% Input argumuents:
% -------------------------------------------------------------------------
% p0           [14x1 double]     state and costate variables      [ - ]        
% auxdata      [structure]       parameters                       [ - ]
%
% Output argumuents:
% -------------------------------------------------------------------------
% None
%
% External functions called:
% -------------------------------------------------------------------------
% fx_.m
% fy_.m
% Sw_.m
%
% Copyright(C) 2015/06/22 by Chen Zhang, 
% School of Astronautics, Beihang University
% chenzhang.buaa@gmail.com
% -------------------------------------------------------------------------
function plot_orbit_(p0 , auxdata)
LU = auxdata.LU;
TU = auxdata.TU;
VU = auxdata.VU;
MU = auxdata.MU;
mee0 = auxdata.mee0;
meef = auxdata.meef;
options = odeset('RelTol',1e-12,'AbsTol', 1e-12);

% plot figure 1
figure('position' , [100 , 100 , 500 , 500])
% plot initial orbit
[tt0 , yy0] = ode113(@fx_ , [0 , 43200 / TU] , ...
    [mee0(1:6) ; 1], options , auxdata);
[r0, v0] = mee2eci_(auxdata.mu, yy0');
plot3(r0(: , 1) , r0(: , 2) , r0(: , 3) , 'r'); hold on;
% plot final orbit
[ttf , yyf] = ode113(@fx_ , [0 : 1000 / TU : 86400 / TU] , ...
    [meef ; 0 ; 1] , options , auxdata);
[rf, vf] = mee2eci_(auxdata.mu, yyf');
plot3(rf(: , 1) , rf(: , 2) , rf(: , 3) , 'r'); hold on;
% plot transfer orbit
[tt, yy] = ode113(@fy_ , [0 , p0(end)] , ...
    [auxdata.mee0 ; p0(1 : 7)] , options , auxdata);
[rr, vv] = mee2eci_(auxdata.mu, yy');
plot3(rr(: , 1) , rr(: , 2) , rr(: , 3) , 'b' , 'LineWidth' , 2); hold on;
% plot earth
Re = 6378;
[xs, ys, zs] = sphere(10) ;
surf(xs*Re/LU,  ys*Re/LU, zs*Re/LU, 'FaceColor', 'w') ; hold on;
axis equal;
view(0 , 90);

% plot figure 2
figure('position' , [150 , 150 , 500 , 400])
Sw_array = zeros(size(tt));
u_array = zeros(size(tt));
H_array = zeros(size(tt));
for k = 1 : size(yy , 1)
    [Sw , A , ~] = Sw_(yy(k , :) , auxdata);
    if Sw < 0
        u = 1;
    else
        u = 0;
    end
    u_array(k) = u;
    Sw_array(k) = Sw;
    H_array(k) = 1 + yy(k , 8 : 13) * A + u * Sw;
end
subplot(4,1,1)
plot(tt , u_array , 'r' , 'LineWidth' , 2); hold on;
ylabel('u');
subplot(4,1,2)
plot(tt , Sw_array , 'b' , 'LineWidth' , 2); hold on;
ylabel('S');
subplot(4,1,3)
plot(tt , yy(: , 7) , 'b' , 'LineWidth' , 2); hold on;
ylabel('m');
subplot(4,1,4)
plot(tt , H_array , 'b' , 'LineWidth' , 2); hold on;
ylabel('Hamiltonian');
xlabel('time(TU)');
end
% -------------------------------------------------------------------------
